<pre class="code">
<span class="srcline"><span class="lineno"><a href="17,1" id="srcline1"> 1</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="17,2" id="srcline2"> 2</a></span><span class="line"><span class="comment">% Creates a body struct with default body properties</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,3" id="srcline3"> 3</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="17,4" id="srcline4"> 4</a></span><span class="line"><span class="keyword">function</span> <span class="var type1" id="S2T17U3">B</span> = Body()</span></span>
<span class="srcline"><span class="lineno"><a href="17,5" id="srcline5"> 5</a></span><span class="line"> <span class="comment">%#codegen</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,6" id="srcline6"> 6</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="17,7" id="srcline7"> 7</a></span><span class="line">    <span class="comment">% The body struct</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,8" id="srcline8"> 8</a></span><span class="line">    <span class="comment">%B = struct;</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,9" id="srcline9"> 9</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="17,10" id="srcline10">10</a></span><span class="line">    <span class="mxinfo" id="T18:U2"><span class="mxinfo" id="T18:U3"><span class="var type1" id="S2T17U9">B</span>.name</span> = <span class="mxinfo" id="T18:U5"><span class="string">'body'</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="17,11" id="srcline11">11</a></span><span class="line">    <span class="mxinfo" id="T19:U6"><span class="mxinfo" id="T19:U7"><span class="var type1" id="S2T17U15">B</span>.type</span> = <span class="mxinfo" id="T19:U9"><span class="string">'generic'</span></span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="17,12" id="srcline12">12</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="17,13" id="srcline13">13</a></span><span class="line">    <span class="mxinfo" id="T1:U10"><span class="mxinfo" id="T1:U11"><span class="var type1" id="S2T17U21">B</span>.bodyID</span> = <span class="mxinfo" id="T1:U13">0</span></span>;           <span class="comment">% Body's position in the simulation's vector of bodies</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,14" id="srcline14">14</a></span><span class="line">    <span class="mxinfo" id="T1:U14"><span class="mxinfo" id="T1:U15"><span class="var type1" id="S2T17U27">B</span>.bodyContactID</span> = <span class="mxinfo" id="T1:U17">0</span></span>;    <span class="comment">% Body's id for current time-step contact set</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,15" id="srcline15">15</a></span><span class="line">    <span class="mxinfo" id="T1:U18"><span class="mxinfo" id="T1:U19"><span class="var type1" id="S2T17U33">B</span>.bodyDynamicID</span> = <span class="mxinfo" id="T1:U21">0</span></span>;    <span class="comment">% TODO: duplicate of bodyContactID? </span></span></span>
<span class="srcline"><span class="lineno"><a href="17,16" id="srcline16">16</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="17,17" id="srcline17">17</a></span><span class="line">    <span class="comment">% Physical properties</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,18" id="srcline18">18</a></span><span class="line">    <span class="mxinfo" id="T4:U22"><span class="mxinfo" id="T4:U23"><span class="var type1" id="S2T17U39">B</span>.dynamic</span> = <span class="mxinfo" id="T4:U25">true</span></span>;       <span class="comment">% Dynamic vs. static body </span></span></span>
<span class="srcline"><span class="lineno"><a href="17,19" id="srcline19">19</a></span><span class="line">    <span class="mxinfo" id="T4:U26"><span class="mxinfo" id="T4:U27"><span class="var type1" id="S2T17U46">B</span>.active</span> = <span class="mxinfo" id="T4:U29">false</span></span>;       <span class="comment">% An active body is one that is in dynamic interaction with </span></span></span>
<span class="srcline"><span class="lineno"><a href="17,20" id="srcline20">20</a></span><span class="line">                            <span class="comment">%   another body through contact or some other constraint.  </span></span></span>
<span class="srcline"><span class="lineno"><a href="17,21" id="srcline21">21</a></span><span class="line">    <span class="mxinfo" id="T1:U30"><span class="mxinfo" id="T1:U31"><span class="var type1" id="S2T17U53">B</span>.mass</span> = <span class="mxinfo" id="T1:U33">1</span></span>;             <span class="comment">% Mass</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,22" id="srcline22">22</a></span><span class="line">    <span class="mxinfo" id="T7:U34"><span class="mxinfo" id="T7:U35"><span class="var type1" id="S2T17U59">B</span>.u</span> = <span class="mxinfo" id="T7:U37">zeros(<span class="mxinfo" id="T1:U38">3</span>,1)</span></span>;       <span class="comment">% Position of center of mass</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,23" id="srcline23">23</a></span><span class="line">    <span class="mxinfo" id="T20:U39"><span class="mxinfo" id="T20:U40"><span class="var type1" id="S2T17U68">B</span>.quat</span> = <span class="mxinfo" id="T20:U42">[<span class="mxinfo" id="T1:U43">1</span>;<span class="mxinfo" id="T1:U44">0</span>;<span class="mxinfo" id="T1:U45">0</span>;<span class="mxinfo" id="T1:U46">0</span>]</span></span>;     <span class="comment">% Quaternion rotation</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,24" id="srcline24">24</a></span><span class="line">    <span class="mxinfo" id="T16:U47"><span class="mxinfo" id="T16:U48"><span class="var type1" id="S2T17U82">B</span>.J</span> = <span class="mxinfo" id="T16:U50">eye(3)</span></span>;           <span class="comment">% Inertia tensor</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,25" id="srcline25">25</a></span><span class="line">    <span class="comment">%B.v = zeros(3,1);       % Linear velocity</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,26" id="srcline26">26</a></span><span class="line">    <span class="comment">%B.w = zeros(3,1);       % Angular velocity</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,27" id="srcline27">27</a></span><span class="line">    <span class="mxinfo" id="T21:U51"><span class="mxinfo" id="T21:U52"><span class="var type1" id="S2T17U90">B</span>.nu</span> = <span class="mxinfo" id="T21:U54">zeros(<span class="mxinfo" id="T1:U55">6</span>,1)</span></span>;      <span class="comment">% Generalized velocity [v; w]</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,28" id="srcline28">28</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="17,29" id="srcline29">29</a></span><span class="line">    <span class="mxinfo" id="T21:U56"><span class="mxinfo" id="T21:U57"><span class="var type1" id="S2T17U99">B</span>.Fext</span> = <span class="mxinfo" id="T21:U59">zeros(<span class="mxinfo" id="T1:U60">6</span>,1)</span></span>;    <span class="comment">% External force</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,30" id="srcline30">30</a></span><span class="line">    <span class="mxinfo" id="T21:U61"><span class="mxinfo" id="T21:U62"><span class="var type1" id="S2T17U108">B</span>.Aext</span> = <span class="mxinfo" id="T21:U64">zeros(<span class="mxinfo" id="T1:U65">6</span>,1)</span></span>;    <span class="comment">% External acceleration</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,31" id="srcline31">31</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="17,32" id="srcline32">32</a></span><span class="line">    <span class="mxinfo" id="T1:U66"><span class="mxinfo" id="T1:U67"><span class="var type1" id="S2T17U117">B</span>.mu</span> = <span class="mxinfo" id="T1:U69">0.5</span></span>;             <span class="comment">% Friction factor</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,33" id="srcline33">33</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="17,34" id="srcline34">34</a></span><span class="line">    <span class="comment">% Joint-related properties</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,35" id="srcline35">35</a></span><span class="line">    <span class="mxinfo" id="T1:U70"><span class="mxinfo" id="T1:U71"><span class="var type1" id="S2T17U123">B</span>.numJoints</span> = <span class="mxinfo" id="T1:U73">0</span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="17,36" id="srcline36">36</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="17,37" id="srcline37">37</a></span><span class="line">    <span class="comment">% Graphics properties </span></span></span>
<span class="srcline"><span class="lineno"><a href="17,38" id="srcline38">38</a></span><span class="line">    <span class="mxinfo" id="T4:U74"><span class="mxinfo" id="T4:U75"><span class="var type1" id="S2T17U129">B</span>.visible</span> = <span class="mxinfo" id="T4:U77">true</span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="17,39" id="srcline39">39</a></span><span class="line">    <span class="mxinfo" id="T9:U78"><span class="mxinfo" id="T9:U79"><span class="var type1" id="S2T17U136">B</span>.graphicsHandle</span> = <span class="mxinfo" id="T9:U81">[]</span></span>;  <span class="comment">% Graphics object for plotting the body</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,40" id="srcline40">40</a></span><span class="line">    <span class="mxinfo" id="T9:U82"><span class="mxinfo" id="T9:U83"><span class="var type1" id="S2T17U143">B</span>.Xdata</span> = <span class="mxinfo" id="T9:U85">[]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="17,41" id="srcline41">41</a></span><span class="line">    <span class="mxinfo" id="T9:U86"><span class="mxinfo" id="T9:U87"><span class="var type1" id="S2T17U150">B</span>.Ydata</span> = <span class="mxinfo" id="T9:U89">[]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="17,42" id="srcline42">42</a></span><span class="line">    <span class="mxinfo" id="T9:U90"><span class="mxinfo" id="T9:U91"><span class="var type1" id="S2T17U157">B</span>.Zdata</span> = <span class="mxinfo" id="T9:U93">[]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="17,43" id="srcline43">43</a></span><span class="line">    <span class="mxinfo" id="T22:U94"><span class="mxinfo" id="T22:U95"><span class="var type1" id="S2T17U164">B</span>.color</span> = <span class="mxinfo" id="T22:U97">[<span class="mxinfo" id="T1:U98">0</span> <span class="mxinfo" id="T1:U99">0</span> <span class="mxinfo" id="T1:U100">1</span>]</span></span>;      <span class="comment">% Color of body</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,44" id="srcline44">44</a></span><span class="line">    <span class="mxinfo" id="T1:U101"><span class="mxinfo" id="T1:U102"><span class="var type1" id="S2T17U174">B</span>.facealpha</span> = <span class="mxinfo" id="T1:U104">0.7</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="17,45" id="srcline45">45</a></span><span class="line">    <span class="mxinfo" id="T1:U105"><span class="mxinfo" id="T1:U106"><span class="var type1" id="S2T17U180">B</span>.edgealpha</span> = <span class="mxinfo" id="T1:U108">1</span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="17,46" id="srcline46">46</a></span><span class="line">    <span class="mxinfo" id="T9:U109"><span class="mxinfo" id="T9:U110"><span class="var type1" id="S2T17U186">B</span>.bboxHandle</span> = <span class="mxinfo" id="T9:U112">[]</span></span>;      <span class="comment">% Graphics object for plotting bounding box</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,47" id="srcline47">47</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="17,48" id="srcline48">48</a></span><span class="line">    <span class="comment">% Collision detection properties</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,49" id="srcline49">49</a></span><span class="line">    <span class="mxinfo" id="T22:U113"><span class="mxinfo" id="T22:U114"><span class="var type1" id="S2T17U193">B</span>.AABB_min</span> = <span class="mxinfo" id="T22:U116">zeros(<span class="mxinfo" id="T1:U117">1</span>,<span class="mxinfo" id="T1:U118">3</span>)</span></span>; <span class="comment">% Bounding box</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,50" id="srcline50">50</a></span><span class="line">    <span class="mxinfo" id="T22:U119"><span class="mxinfo" id="T22:U120"><span class="var type1" id="S2T17U202">B</span>.AABB_max</span> = <span class="mxinfo" id="T22:U122">zeros(<span class="mxinfo" id="T1:U123">1</span>,<span class="mxinfo" id="T1:U124">3</span>)</span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="17,51" id="srcline51">51</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="17,52" id="srcline52">52</a></span><span class="line">    <span class="mxinfo" id="T4:U125"><span class="mxinfo" id="T4:U126"><span class="var type1" id="S2T17U211">B</span>.collides</span> = <span class="mxinfo" id="T4:U128">true</span></span>;         <span class="comment">% If false, this body won't collide with any other bodies</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,53" id="srcline53">53</a></span><span class="line">    <span class="mxinfo" id="T9:U129"><span class="mxinfo" id="T9:U130"><span class="var type1" id="S2T17U218">B</span>.doesNotCollideWith</span> = <span class="mxinfo" id="T9:U132">[]</span></span>; <span class="comment">% A list of bodyIDs of bodies not to do collision detection with</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,54" id="srcline54">54</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="17,55" id="srcline55">55</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="17,56" id="srcline56">56</a></span><span class="line">    <span class="comment">%% Different bodies have different attributes, but for simplicity </span></span></span>
<span class="srcline"><span class="lineno"><a href="17,57" id="srcline57">57</a></span><span class="line">    <span class="comment">%  we want all bodies to be represented by this struct.  Therefore we</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,58" id="srcline58">58</a></span><span class="line">    <span class="comment">%  place all body attributes here, and let bodies pick and choose.</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,59" id="srcline59">59</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="17,60" id="srcline60">60</a></span><span class="line">    <span class="comment">%% Sphere attributes</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,61" id="srcline61">61</a></span><span class="line">    <span class="mxinfo" id="T1:U133"><span class="mxinfo" id="T1:U134"><span class="var type1" id="S2T17U225">B</span>.radius</span> = <span class="mxinfo" id="T1:U136">1</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="17,62" id="srcline62">62</a></span><span class="line">    <span class="mxinfo" id="T1:U137"><span class="mxinfo" id="T1:U138"><span class="var type1" id="S2T17U231">B</span>.num_sphere_verts</span> = <span class="mxinfo" id="T1:U140">7</span></span>;  <span class="comment">% Decrease to improve graphics performance </span></span></span>
<span class="srcline"><span class="lineno"><a href="17,63" id="srcline63">63</a></span><span class="line">                             <span class="comment">% (does not affect simulation result)</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,64" id="srcline64">64</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="17,65" id="srcline65">65</a></span><span class="line">    <span class="comment">%% Cylinder attributes</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,66" id="srcline66">66</a></span><span class="line">    <span class="mxinfo" id="T1:U141"><span class="mxinfo" id="T1:U142"><span class="var type1" id="S2T17U237">B</span>.height</span> = <span class="mxinfo" id="T1:U144">1</span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="17,67" id="srcline67">67</a></span><span class="line">                             </span></span>
<span class="srcline"><span class="lineno"><a href="17,68" id="srcline68">68</a></span><span class="line">    <span class="comment">%% Plane attributes</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,69" id="srcline69">69</a></span><span class="line">    <span class="mxinfo" id="T22:U145"><span class="mxinfo" id="T22:U146"><span class="var type1" id="S2T17U243">B</span>.plane_normal</span> = <span class="mxinfo" id="T22:U148">[<span class="mxinfo" id="T1:U149">0</span> <span class="mxinfo" id="T1:U150">0</span> <span class="mxinfo" id="T1:U151">1</span>]</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="17,70" id="srcline70">70</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="17,71" id="srcline71">71</a></span><span class="line">    <span class="comment">%% Mesh attributes</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,72" id="srcline72">72</a></span><span class="line">    <span class="mxinfo" id="T1:U152"><span class="mxinfo" id="T1:U153"><span class="var type1" id="S2T17U253">B</span>.num_verts</span> = <span class="mxinfo" id="T1:U155">0</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="17,73" id="srcline73">73</a></span><span class="line">    <span class="mxinfo" id="T1:U156"><span class="mxinfo" id="T1:U157"><span class="var type1" id="S2T17U259">B</span>.num_edges</span> = <span class="mxinfo" id="T1:U159">0</span></span>;</span></span>
<span class="srcline"><span class="lineno"><a href="17,74" id="srcline74">74</a></span><span class="line">    <span class="mxinfo" id="T1:U160"><span class="mxinfo" id="T1:U161"><span class="var type1" id="S2T17U265">B</span>.num_faces</span> = <span class="mxinfo" id="T1:U163">0</span></span>; </span></span>
<span class="srcline"><span class="lineno"><a href="17,75" id="srcline75">75</a></span><span class="line"></span></span>
<span class="srcline"><span class="lineno"><a href="17,76" id="srcline76">76</a></span><span class="line">    <span class="mxinfo" id="T9:U164"><span class="mxinfo" id="T9:U165"><span class="var type1" id="S2T17U271">B</span>.verts_local</span> = <span class="mxinfo" id="T9:U167">[]</span></span>;  <span class="comment">% A Nx3 vector of N vertices relative to the COM of the body</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,77" id="srcline77">77</a></span><span class="line">    <span class="mxinfo" id="T9:U168"><span class="mxinfo" id="T9:U169"><span class="var type1" id="S2T17U278">B</span>.verts_world</span> = <span class="mxinfo" id="T9:U171">[]</span></span>;  <span class="comment">% A Nx3 vector of N vertices in the world space</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,78" id="srcline78">78</a></span><span class="line">    <span class="mxinfo" id="T9:U172"><span class="mxinfo" id="T9:U173"><span class="var type1" id="S2T17U285">B</span>.edges</span> = <span class="mxinfo" id="T9:U175">[]</span></span>;        <span class="comment">% A Ex [v1 v2 Enext Eopp face]   TODO: remove implicit columns Enext and face</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,79" id="srcline79">79</a></span><span class="line">                            <span class="comment">% A Ex5 vector [v1 v2 f1 f2 a] where v1:first</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,80" id="srcline80">80</a></span><span class="line">                            <span class="comment">% vert, v2: second vert, f1: first face, f2:</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,81" id="srcline81">81</a></span><span class="line">                            <span class="comment">% second face, a: angle between faces</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,82" id="srcline82">82</a></span><span class="line">    <span class="mxinfo" id="T9:U176"><span class="mxinfo" id="T9:U177"><span class="var type1" id="S2T17U292">B</span>.tvecs</span> = <span class="mxinfo" id="T9:U179">[]</span></span>;        <span class="comment">% A Ex6 vector of t vectors (always in local coords), perp to edge and planar to adjacent faces</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,83" id="srcline83">83</a></span><span class="line">    <span class="mxinfo" id="T9:U180"><span class="mxinfo" id="T9:U181"><span class="var type1" id="S2T17U299">B</span>.faces</span> = <span class="mxinfo" id="T9:U183">[]</span></span>;        <span class="comment">% A Mx4 vector of M faces where each row contains: [v1 v2 v3 e1]</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,84" id="srcline84">84</a></span><span class="line">    <span class="mxinfo" id="T9:U184"><span class="mxinfo" id="T9:U185"><span class="var type1" id="S2T17U306">B</span>.face_norms</span> = <span class="mxinfo" id="T9:U187">[]</span></span>;   <span class="comment">% A Mx3 vector of M face norms where each row contains [n1 n2 n3] </span></span></span>
<span class="srcline"><span class="lineno"><a href="17,85" id="srcline85">85</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="17,86" id="srcline86">86</a></span><span class="line">    </span></span>
<span class="srcline"><span class="lineno"><a href="17,87" id="srcline87">87</a></span><span class="line"><span class="keyword">end</span></span></span>
<span class="srcline"><span class="lineno"><a href="17,88" id="srcline88">88</a></span><span class="line"></span></span>
</pre>
